//------------------------------------------------------------------------------
//  <copyright file="LocalizationParticleFilter.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Localization
{
    using System;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using MicrosoftResearch.Infer.Distributions;

    /// <summary>
    /// Localization particle filter. Each particle maintains a its pose on the global map.  
    /// </summary>
    /// <typeparam name="TDriveFeedbackState">Drive feedback state which provides the motion model</typeparam>
    public class LocalizationParticleFilter<TDriveFeedbackState> : ParticleFilter<SLAM2DParticle, TDriveFeedbackState, KinectHorizontalDepthProfile> where TDriveFeedbackState : IDriveState
    {
        /// <summary>
        /// The translation variance used for replacement resampling
        /// </summary>
        private const double TranslationVariance = 0.1;

        /// <summary>
        /// The rotation variance used for replacement resampling
        /// </summary>
        private const double RotationVariance = 0.1 * MathConstants.Degrees2Radians;

        /// <summary>
        /// Constant noise added to each sample on top of the odometry noise
        /// </summary>
        private const double AdditionalNoiseTranslationVariance = 0.03 * 0.03;

        /// <summary>
        /// Constant noise added to each sample on top of the odometry noise
        /// </summary>
        private const double AdditionalNoiseRotationVariance = 0.003 * 0.003;
            
        /// <summary>
        /// The map we localize on.
        /// </summary>
        private GlobalMap map;

        /// <summary>
        /// The likely start pose
        /// </summary>
        private Pose2D startPose;

        /// <summary>
        /// The ambiguity in the start pose location.
        /// </summary>
        private Pose2D startPoseVariance;

        /// <summary>
        /// The pose of the best particle after the last filter update 
        /// </summary>
        private Pose2D priorHighestLikelyhood;

        /// <summary>
        /// Initializes a new instance of the LocalizationParticleFilter class.
        /// </summary>
        /// <param name="map">The map we localize on.</param>
        /// <param name="particleCount">The count of particles to maintain.</param>
        /// <param name="startPose">The likely start pose</param>
        /// <param name="startPoseVariance">The ambiguity in the placement of the start pose.</param>
        public LocalizationParticleFilter(GlobalMap map, int particleCount, Pose2D startPose, Pose2D startPoseVariance)
            : base(particleCount, "LocalizationParticleFilter")
        {
            this.EnableRandomResampling = true;
            this.map = map;
            this.map.EnableCaching();
            
            this.startPose = startPose;
            this.startPoseVariance = startPoseVariance;
            this.ResampleAll();
        }

        /// <summary>
        /// Gets the map we localize on.
        /// </summary>
        public GlobalMap Map
        {
            get { return this.map; } 
        }

        /// <summary>
        /// Updates all particles based on the specified motion update and observation. 
        /// </summary>
        /// <param name="update">The motion update to apply to all particles.</param>
        /// <param name="observation">The observation made after the motion completed. This observation is used to determine the fitness of each particle.</param>
        public override void Update(TDriveFeedbackState update, KinectHorizontalDepthProfile observation)
        {
            base.Update(update, observation);
            this.priorHighestLikelyhood = this.GetHighestLikelihoodParticle().Pose;
        }

        /// <summary>
        /// Clones an existing sample.
        /// </summary>
        /// <param name="priorState">The existing particle.</param>
        /// <returns>A clone of the existing particle.</returns>
        protected override SLAM2DParticle CloneSample(SLAM2DParticle priorState)
        {
            SLAM2DParticle newParticle = priorState.Clone(false /*don't make a copy of the map*/);
            return newParticle;
        }

        /// <summary>
        /// Updates the given particle based on motion and observation and returns the probability that the state specified by the particle would result in the given observation.
        /// </summary>
        /// <param name="priorState">The old particle state.</param>
        /// <param name="update">The update to apply.</param>
        /// <param name="observation">The observation, assumed noisy.</param>
        /// <param name="weight">The current weight of the particle</param>
        /// <param name="fitnessToObservation">The probability of a fit, in log-likelihood form.</param>
        /// <returns>The updated particle.</returns>
        protected override SLAM2DParticle UpdateSample(
            SLAM2DParticle priorState,
            TDriveFeedbackState update, 
            KinectHorizontalDepthProfile observation, 
            double weight, 
            out double fitnessToObservation)
        {
            Pose2D newPose = priorState.Pose;
            TDriveFeedbackState previousDriveFeedbackState = (TDriveFeedbackState)priorState.DriveFeedbackState;
            if (previousDriveFeedbackState != null)
            {                
                Pose2D previousPoseInMeters = this.map.MapCellsToMeters(priorState.Pose);
                Pose2D updatedPoseInMeters = update.SamplePose(previousPoseInMeters, previousDriveFeedbackState);
                newPose = this.map.MetersToMapCells(updatedPoseInMeters);
                newPose = this.CreateRandomSample(newPose, new Pose2D(AdditionalNoiseTranslationVariance, AdditionalNoiseTranslationVariance, AdditionalNoiseRotationVariance));
            }

            // make sure we are moving in open space, i.e. far enough from obstacles
            // if we are too close to obstacles on the map, we'll asume we are not really moving
            if (this.map.IsFree((int)newPose.X, (int)newPose.Y))
            {
                priorState.Pose = newPose;
            }
            else
            {
                // resample at random, since we shouldn't be over an obstacle, and adopt the new pose if the random resampler gave us a valid one
                newPose = this.CreateRandomSample(this.priorHighestLikelyhood, this.startPoseVariance);

                if (this.map.IsFree((int)newPose.X, (int)newPose.Y))
                {
                    priorState.Pose = newPose;
                }
            }

            // compute the fitness
            IHorizontalDepthProfile surface;
            Pose2D pose = priorState.Pose;
            if (priorState.UpdateCount % 1 == 0)
            {
                fitnessToObservation = this.map.Match(observation, ref pose, false /*allowExploration*/, out surface);
                priorState.SurfaceFromMap = surface;
            }
            else
            {
                fitnessToObservation = this.map.ApproximateMatch(observation, pose);
            }

            // record the properties used in visualization 
            priorState.SingleIterationWeight = fitnessToObservation;
            priorState.Weight = weight;
            priorState.UpdateCount++;
            priorState.DriveFeedbackState = update;
            return priorState;
        }

        /// <summary>
        /// Creates a random initial sample drawn from the unoccupied cells. 
        /// </summary>
        /// <returns>A random sample drawn from the state space.</returns>
        protected override SLAM2DParticle CreateRandomSample()
        {
            Pose2D pose = this.CreateRandomSample(this.startPose, this.startPoseVariance);
            return new SLAM2DParticle(this.map, pose);
        }

        /// <summary>
        /// Returns a sample from the state space that matches the observation.
        /// The default behavior is to ignore the observation and return a random sample.
        /// </summary>
        /// <param name="priorState">The old particle state.</param>
        /// <param name="update">The update to apply.</param>
        /// <param name="observation">The observation to match.</param>
        /// <param name="weight">The current weight of the particle</param>
        /// <param name="fitnessToObservation">The probability of a fit, in log-likelihood form.</param>
        /// <returns>A sample from the state space.</returns>
        protected override SLAM2DParticle CreateRandomSampleFromPriorAndObservation(
            SLAM2DParticle priorState,
            TDriveFeedbackState update,
            KinectHorizontalDepthProfile observation, 
            double weight, 
            out double fitnessToObservation)
        {
            // pick a sample using a large gaussian distribution around the prior state
            Pose2D pose = this.CreateRandomSample(this.priorHighestLikelyhood, new Pose2D(TranslationVariance, TranslationVariance, RotationVariance));
            
            IHorizontalDepthProfile surface;
            fitnessToObservation = this.map.Match(observation, ref pose, false /*allowExploration*/, out surface);
            priorState.Pose = pose;

            // record the properties used in visualization 
            priorState.SingleIterationWeight = fitnessToObservation;
            priorState.Weight = weight;
            priorState.UpdateCount++;
            priorState.SurfaceFromMap = surface;
            return priorState;
        }

        /// <summary>
        /// Creates a random initial sample drawn from the unoccupied cells. 
        /// </summary>
        /// <param name="priorPose">The pose to sample around</param>
        /// <param name="poseVariance">The variance to use on x, y and heading</param>
        /// <returns>A random sample drawn from the state space.</returns>
        private Pose2D CreateRandomSample(Pose2D priorPose, Pose2D poseVariance)
        {
            const int MaxIterations = 640;
            Gaussian gaussianX = new Gaussian(priorPose.X, poseVariance.X);
            Gaussian gaussianY = new Gaussian(priorPose.Y, poseVariance.Y);
            WrappedGaussian gaussianHeading = new WrappedGaussian(priorPose.Heading, poseVariance.Heading, 2 * Math.PI);

            double candidateX = priorPose.X;
            double candidateY = priorPose.Y;
            double orientation = priorPose.Heading;
            for (int i = 0; i < MaxIterations; i++)
            {
                double x = gaussianX.Sample();
                double y = gaussianY.Sample();
                if (x > 0 && x < this.map.Height && y > 0 && y < this.map.Width && this.map.IsFree((int)x, (int)y))
                {
                    candidateX = x;
                    candidateY = y;
                    orientation = gaussianHeading.Sample();
                    break;
                }
            }

            Pose2D newPose = new Pose2D(candidateX, candidateY, orientation);
            return newPose;
        }
    }
}
